#include <ace/config.h>
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <vector>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>

#include <yarp/sig/all.h>
#include <yarp/os/all.h>
#include <yarp/dev/all.h>

#include <cv.h>
#include <cvaux.h>
#include <highgui.h>

using namespace std;

using namespace yarp::dev;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::sig::draw;
using namespace yarp::sig::file;

int main (int argc, char *argv [])
{
  int i;
  int count;
  int c;
  
  ostringstream imageNameLeft, imageNameRight;
  
  vector<ImageOf<PixelRgb> > imagesBuffLeft, imagesBuffRight;
  
  BufferedPort<ImageOf<PixelRgb> > imagePortLeft;
  BufferedPort<ImageOf<PixelRgb> > imagePortRight;
  
  srand(time(NULL));
  // connect to the head
  Network::init();
  
  imagePortLeft.open ("/image/left/in"); 
  imagePortRight.open ("/image/right/in"); 

  Network::connect ("/icub/cam/left", "/image/left/in");
  Network::connect ("/icub/cam/right", "/image/right/in");
  
  cvNamedWindow ("Egidio bell'uomo", CV_WINDOW_AUTOSIZE);
  
  cout << "Start acquiring: " << endl;
  
  c = 'z';
  count = 0;
  while (c != 'q')
  {
    
    if (c == 'a')
    {
      imagesBuffLeft.push_back (*(imagePortLeft.read ()));
      imagesBuffRight.push_back (*(imagePortRight.read ()));
      cout << "images number " << ++count << endl; 
    }
      
    c = cvWaitKey ();
  }
  
  cout << "End acquiring." << endl;
  
  cout << "Start saving" << endl;

  for (i = 0; i < imagesBuffLeft.size (); i++)
  { 
    imageNameLeft << "/home/nigno/Immagini/dataset/Left" << i << ".ppm";
    yarp::sig::file::write (imagesBuffLeft [i], imageNameLeft.str ().c_str ());
    imageNameLeft.seekp (0);
    
    imageNameRight << "/home/nigno/Immagini/dataset/Right" << i << ".ppm";
    yarp::sig::file::write (imagesBuffRight [i], imageNameRight.str ().c_str ());
    imageNameRight.seekp (0);
  }
  
  cout << "End saving" << endl;
  
  cvDestroyAllWindows ();
  
  return 0;
}
